/*
 * heightfield.cpp
 *
 *  Created on: 17.09.2013
 *      Author: alex
 */
#include <bullet_rviz_test/heightfield.h>
#include <math.h>
#include <bullet/btBulletDynamicsCommon.h>
#include <bullet/btBulletCollisionCommon.h>
#include <bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
#include <bullet_rviz_test/physics.h>

namespace bullet_rviz_test
{

Heightfield::Heightfield(const btVector3 & extents, const float * data,
    size_t size_x, size_t size_y) :
    extents_(extents), size_x_(size_x), size_y_(size_y)
{
  data_ = new float[size_x_ * size_y_];
  for (size_t i = 0; i < size_x_ * size_y_; ++i)
    data_[i] = data[i];

  heightfield_terrain_shape_ = new btHeightfieldTerrainShape(size_x_, size_y_,
      data_, 1.0f, -extents.z(), extents.z(), 2, PHY_FLOAT, false);
  heightfield_terrain_shape_->setLocalScaling(
      btVector3(extents_.x() / (size_x_ - 1U), extents_.y() / (size_y_ - 1U),
          1.0f));

  motion_state_ = new MotionState(this,
      btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)));

  rigid_body_ = new btRigidBody(
      btRigidBody::btRigidBodyConstructionInfo(0, motion_state_,
          heightfield_terrain_shape_, btVector3(0, 0, 0)));
}

Heightfield::~Heightfield()
{
  delete rigid_body_;
  delete motion_state_;
  delete heightfield_terrain_shape_;
  delete[] data_;
}

btVector3 Heightfield::getExtents()
{
  return extents_;
}

const float * Heightfield::getData()
{
  return data_;
}

float Heightfield::getData(size_t x_index, size_t y_index)
{
  return data_[y_index * size_x_ + x_index];
}

float Heightfield::getHeightAtPosition(btScalar x, btScalar y)
{
  btVector3 position = rigid_body_->getWorldTransform().getOrigin();
  btScalar px0 = position.x() - extents_.x() / 2;
  btScalar px1 = position.x() + extents_.x() / 2;
  btScalar py0 = position.y() - extents_.y() / 2;
  btScalar py1 = position.y() + extents_.y() / 2;

  if (px0 <= x && x <= px1 && py0 <= y && y <= py1)
  {
    size_t x_index = static_cast<size_t>((size_x_ - 1) * (x - px0) / (px1 - px0));
    size_t y_index = static_cast<size_t>((size_y_ - 1) * (y - py0) / (py1 - py0));
    if (x_index >= (size_x_ - 1))
      x_index = size_x_ - 2;
    if (y_index >= (size_y_ - 1))
      y_index = size_y_ - 2;

    btScalar x0 = px0 + extents_.x() * x_index / (size_x_ - 1);
    btScalar y0 = py0 + extents_.y() * y_index / (size_y_ - 1);
    btScalar x1 = px0 + extents_.x() * (x_index + 1U) / (size_x_ - 1);
    btScalar y1 = py0 + extents_.y() * (y_index + 1U) / (size_y_ - 1);

    float z00 = getData(x_index, y_index);
    float z01 = getData(x_index + 1, y_index);
    float z10 = getData(x_index, y_index + 1);
    float z11 = getData(x_index + 1, y_index + 1);

    float za = z00 + (z01 - z00) * (x - x0) / (x1 - x0);
    float zb = z10 + (z11 - z10) * (x - x0) / (x1 - x0);

    return position.z() + za + (zb - za) * (y - y0) / (y1 - y0);
  }

  return NAN;
}

size_t Heightfield::getSizeX()
{
  return size_x_;
}

size_t Heightfield::getSizeY()
{
  return size_y_;
}

btMotionState * Heightfield::getMotionState()
{
  return motion_state_;
}

btRigidBody * Heightfield::getRigidBody()
{
  return rigid_body_;
}

}
